#include "robotdriver.h"

#define arm_joint1_channel      0
#define arm_joint2_channel      1
#define arm_joint3_channel      2
#define arm_joint4_channel      4
#define arm_joint5_channel      5
#define arm_joint6_channel      6

#define set_joint1_degree(degree)  setServoDegree(arm_joint1_channel, degree*180/PI)
#define set_joint2_degree(degree)  setServoDegree(arm_joint2_channel, degree*180/PI)
#define set_joint3_degree(degree)  setServoDegree(arm_joint3_channel, degree*180/PI)
#define set_joint4_degree(degree)  setServoDegree(arm_joint4_channel, degree*180/PI)
#define set_joint5_degree(degree)  setServoDegree(arm_joint5_channel, degree*180/PI)
#define set_joint6_degree(degree)  setServoDegree(arm_joint6_channel, degree*180/PI)

// #define set_joint1_degree(degree)  0
// #define set_joint2_degree(degree)  0
// #define set_joint3_degree(degree)  0
// #define set_joint4_degree(degree)  0
// #define set_joint5_degree(degree)  0
// #define set_joint6_degree(degree)  0

Joints armJointStatus;//global value, record robotic joints
static int ArmReadyFlag = 0;
/**************************************************************************/
void robotdriver_init(){
    robot_init();
    return;
}
/**************************************************************************/
static void print_joint_status(){
    printf("armJointStatus joint theta1 value = %f\n", armJointStatus.theta1);
    printf("armJointStatus joint theta2 value = %f\n", armJointStatus.theta2);
    printf("armJointStatus joint theta3 value = %f\n", armJointStatus.theta3);
    printf("armJointStatus joint theta4 value = %f\n", armJointStatus.theta4);
    printf("armJointStatus joint theta5 value = %f\n", armJointStatus.theta5);
    printf("armJointStatus joint theta6 value = %f\n", armJointStatus.theta6);
    printf("================================================================\n");
}
/**************************************************************************/
/**************************************************************************/
static void Joints2ServoJoint(Joints* joints, ServoJoint* servo_joints){
    servo_joints->theta1 = joints->theta1 * 1;
    servo_joints->theta2 = joints->theta2 * 1;
    servo_joints->theta3 = joints->theta3 * 1;
    servo_joints->theta4 = joints->theta4 * 1;
    servo_joints->theta5 = joints->theta5 * 1;
    servo_joints->theta6 = joints->theta6 * 1;
    return;
}
/**************************************************************************/
void update_joint_status(Joints* joints){
    armJointStatus.theta1 = joints->theta1;
    armJointStatus.theta2 = joints->theta2;
    armJointStatus.theta3 = joints->theta3;
    armJointStatus.theta4 = joints->theta4;
    armJointStatus.theta5 = joints->theta5;
    armJointStatus.theta6 = joints->theta6;
    print_joint_status();
}
/**************************************************************************/
void get_robot_joints(Joints* joints){
    joints->theta1 = armJointStatus.theta1;
    joints->theta2 = armJointStatus.theta2;
    joints->theta3 = armJointStatus.theta3;
    joints->theta4 = armJointStatus.theta4;
    joints->theta5 = armJointStatus.theta5;
    joints->theta6 = armJointStatus.theta6;
    return;
}
void get_robot_joints_list(float* joints_list){
    joints_list[0] = armJointStatus.theta1;
    joints_list[1] = armJointStatus.theta2;
    joints_list[2] = armJointStatus.theta3;
    joints_list[3] = armJointStatus.theta4;
    joints_list[4] = armJointStatus.theta5;
    joints_list[5] = armJointStatus.theta6;
}
/**************************************************************************/
void get_robot_servo_joint(ServoJoint* servo_joints){
    Joints joints;
    get_robot_joints(&joints);
    Joints2ServoJoint(&joints, servo_joints);
    return;
}
/**************************************************************************/
/* move robotic arm to home pose */
void move_to_home(Joints* target_joints){
    //move to home to init armJointStatus
    ServoJoint servo_joints;
    Joints2ServoJoint(target_joints, &servo_joints);
    set_joint1_degree(servo_joints.theta1);
    set_joint2_degree(servo_joints.theta2);
    set_joint3_degree(servo_joints.theta3);
    set_joint4_degree(servo_joints.theta4);
    set_joint5_degree(servo_joints.theta5);
    set_joint6_degree(servo_joints.theta6);
    update_joint_status(target_joints);
    ArmReadyFlag = 1;
return;
}
/**************************************************************************/
int is_robot_ready(void){
    return ArmReadyFlag;
}
/**************************************************************************/
/* move robotic arm by joint */
void moveJ(Joints* target_joints){
    static int max_pose_num = 15;
    Joints start_joints;
    Joints mid_joints_list[max_pose_num+1];
    get_robot_joints(&start_joints);
    int mid_joints_num = jointSpacePlanner(&start_joints, target_joints, mid_joints_list, max_pose_num);
    moveByJoints(mid_joints_list, mid_joints_num);
    return;
}

/**************************************************************************/
/* move the end flange of the robotic arm to another point from current point in a straight line */
void moveL(Pose* target_pose){
    static int max_pose_num = 15;
    Joints start_joints;
    Pose start_pose;
    get_robot_joints(&start_joints);
    forward_kinematics(&start_joints, &start_pose);
    Pose mid_poses[max_pose_num+1];
    int mid_poses_num = linearPlanner(&start_pose, target_pose, mid_poses, max_pose_num);
    moveByPoses(mid_poses, mid_poses_num);
    return;
}
/**************************************************************************/
void moveByJoints(Joints* joints_list, int num){
    int i;
    ServoJoint servo_joints;
    for(i = 1; i < num; i++){
        Joints2ServoJoint(&joints_list[i], &servo_joints);
        set_joint1_degree(servo_joints.theta1);
        set_joint2_degree(servo_joints.theta2);
        set_joint3_degree(servo_joints.theta3);
        set_joint4_degree(servo_joints.theta4);
        set_joint5_degree(servo_joints.theta5);
        set_joint6_degree(servo_joints.theta6);
        update_joint_status(&joints_list[i]);
        print_joint_status();
        //TODO: need time_sleep
    }
    return;
}
/**************************************************************************/
void moveByPoses(Pose* poses_list, const int num){
    int i;
    Joints ik, temp_joints;
    ServoJoint servo_joints;
    for(i = 1; i < num; i++){
        get_robot_joints(&temp_joints);
        get_best_ik(&poses_list[i], &temp_joints, &ik);
        Joints2ServoJoint(&ik, &servo_joints);
        set_joint1_degree(servo_joints.theta1);
        set_joint2_degree(servo_joints.theta2);
        set_joint3_degree(servo_joints.theta3);
        set_joint4_degree(servo_joints.theta4);
        set_joint5_degree(servo_joints.theta5);
        set_joint6_degree(servo_joints.theta6);
        update_joint_status(&ik);
        //TODO: need time_sleep
    }
    return;
}
/**************************************************************************/
//move slightly
static const float small_joint_angle = 3 * PI/ 180; //5 degree 
static const float small_liner_distance = 2;        //2 mm    
void move_joint1_slightly(int direction){
    float step = ((direction == 1) ? small_joint_angle : (-1.0*small_joint_angle));
    Joints joints;
    ServoJoint servo_joints;
    get_robot_joints(&joints);
    joints.theta1 += step;
    Joints2ServoJoint(&joints, &servo_joints);
    set_joint1_degree(servo_joints.theta1);
    update_joint_status(&joints);
}
/**************************************************************************/
void move_joint2_slightly(int direction){
    float step = ((direction == 1) ? small_joint_angle : (-1.0*small_joint_angle));
    Joints joints;
    ServoJoint servo_joints;
    get_robot_joints(&joints);
    joints.theta2 += step;
    Joints2ServoJoint(&joints, &servo_joints);
    set_joint2_degree(servo_joints.theta2);
    update_joint_status(&joints);
}
/**************************************************************************/
void move_joint3_slightly(int direction){
    float step = ((direction == 1) ? small_joint_angle : (-1.0*small_joint_angle));
    Joints joints;
    ServoJoint servo_joints;
    get_robot_joints(&joints);
    joints.theta3 += step;
    Joints2ServoJoint(&joints, &servo_joints);
    set_joint3_degree(servo_joints.theta3);
    update_joint_status(&joints);
}
/**************************************************************************/
void move_joint4_slightly(int direction){
    float step = ((direction == 1) ? small_joint_angle : (-1.0*small_joint_angle));
    Joints joints;
    ServoJoint servo_joints;
    get_robot_joints(&joints);
    joints.theta4 += step;
    Joints2ServoJoint(&joints, &servo_joints);
    set_joint4_degree(servo_joints.theta4);
    update_joint_status(&joints);
}
/**************************************************************************/
void move_joint5_slightly(int direction){
    float step = ((direction == 1) ? small_joint_angle : (-1.0*small_joint_angle));
    Joints joints;
    ServoJoint servo_joints;
    get_robot_joints(&joints);
    joints.theta5 += step;
    Joints2ServoJoint(&joints, &servo_joints);
    set_joint5_degree(servo_joints.theta5);
    update_joint_status(&joints);
}
/**************************************************************************/
void move_joint6_slightly(int direction){
    float step = ((direction == 1) ? small_joint_angle : (-1.0*small_joint_angle));
    Joints joints;
    ServoJoint servo_joints;
    get_robot_joints(&joints);
    joints.theta6 += step;
    Joints2ServoJoint(&joints, &servo_joints);
    set_joint6_degree(servo_joints.theta6);
    update_joint_status(&joints);
}
/**************************************************************************/
void move_x_slightly(int direction){
    float step = ((direction == 1) ? small_liner_distance : (-1.0*small_liner_distance));
    Joints start_joints, ik, servo_joints;
    Pose target_pose;
    get_robot_joints(&start_joints);
    forward_kinematics(&start_joints, &target_pose);
    target_pose.x += step;
    get_best_ik(&target_pose, &start_joints, &ik);
    Joints2ServoJoint(&ik, &servo_joints);
    set_joint1_degree(servo_joints.theta1);
    set_joint2_degree(servo_joints.theta2);
    set_joint3_degree(servo_joints.theta3);
    set_joint4_degree(servo_joints.theta4);
    set_joint5_degree(servo_joints.theta5);
    set_joint6_degree(servo_joints.theta6);
    update_joint_status(&ik);
}
/**************************************************************************/
void move_y_slightly(int direction){
    float step = ((direction == 1) ? small_liner_distance : (-1.0*small_liner_distance));
    Joints start_joints, ik, servo_joints;
    Pose target_pose;
    get_robot_joints(&start_joints);
    forward_kinematics(&start_joints, &target_pose);
    target_pose.y += step;
    get_best_ik(&target_pose, &start_joints, &ik);
    Joints2ServoJoint(&ik, &servo_joints);
    set_joint1_degree(servo_joints.theta1);
    set_joint2_degree(servo_joints.theta2);
    set_joint3_degree(servo_joints.theta3);
    set_joint4_degree(servo_joints.theta4);
    set_joint5_degree(servo_joints.theta5);
    set_joint6_degree(servo_joints.theta6);
    update_joint_status(&ik);
}
/**************************************************************************/
void move_z_slightly(int direction){
    float step = ((direction == 1) ? small_liner_distance : (-1.0*small_liner_distance));
    Joints start_joints, ik, servo_joints;
    Pose target_pose;
    get_robot_joints(&start_joints);
    forward_kinematics(&start_joints, &target_pose);
    target_pose.z += step;
    get_best_ik(&target_pose, &start_joints, &ik);
    Joints2ServoJoint(&ik, &servo_joints);
    set_joint1_degree(servo_joints.theta1);
    set_joint2_degree(servo_joints.theta2);
    set_joint3_degree(servo_joints.theta3);
    set_joint4_degree(servo_joints.theta4);
    set_joint5_degree(servo_joints.theta5);
    set_joint6_degree(servo_joints.theta6);
    update_joint_status(&ik);
}
/**************************************************************************/